% 初始粗对准误差仿真
%
% References:
% [1] 理论文档“解析粗对准姿态误差”

clc;
clear;

load('earthconstants_wgs84', 'omegaIE');
g = 9.8;
L = pi/6;
fL = [0 0 -g]';
omegaIEL = [omegaIE*cos(L) 0 -omegaIE*sin(L)]';
MCNum = 5000;

phiTheory1 = NaN(3, MCNum);
phiActual1 = NaN(3, MCNum);
dOrth1 = NaN(3, MCNum);
dNorm1 = NaN(3, MCNum);
phiTheory2 = NaN(3, MCNum);
phiActual2 = NaN(3, MCNum);
dOrth2 = NaN(3, MCNum);
dNorm2 = NaN(3, MCNum);
dfB = repmat([100 100 100]' * 1e-6 * g, 1, MCNum) .* randn(3, MCNum); % 转换前单位ug
domegaIBB = repmat([0.05 0.05 0.05]' * pi / 180 / 3600, 1, MCNum) .* randn(3, MCNum); % 转换前单位°/h
rot1 = randn(MCNum, 1) * pi;
rot2 = randn(MCNum, 1) * pi;
rot3 = randn(MCNum, 1) * pi;
CBL = angle2dcm(rot1, rot2, rot3);

for i=1:MCNum
    % 解析粗对准
    fHatB = CBL(:, :, i)' * fL + dfB(:, i);
    omegaHatIBB = CBL(:, :, i)' * omegaIEL + domegaIBB(:, i);
    [ CBLHat1 ] = coarsealign_stat_rv1( fHatB, omegaHatIBB, L, g, omegaIE );
    [ CBLHat2 ] = coarsealign_stat_rv2( fHatB, omegaHatIBB );
    
    % 理论误差
    dfL = CBL(:, :, i) * dfB(:, i);
    domegaIBL = CBL(:, :, i) * domegaIBB(:, i);
    phiTheory1(:, i) = coarsealignerr_stat(dfL, domegaIBL, g, omegaIE, L, 1);
    phiTheory2(:, i) = coarsealignerr_stat(dfL, domegaIBL, g, omegaIE, L, 2);
    
    % 实际误差
    [phiActual1(:, i), dOrth1(:, i), dNorm1(:, i)] = dcmdiff(CBLHat1, CBL(:, :, i));
    [phiActual2(:, i), dOrth2(:, i), dNorm2(:, i)] = dcmdiff(CBLHat2, CBL(:, :, i));
end

disp('采用g、ωIE为参考向量的解析粗对准：');
disp('实际姿态误差RMS（″）');
disp(rms(phiActual1, 2) / pi * 180 * 3600);
disp('理论与实际姿态误差差值RMS（″）');
disp(rms(phiActual1-phiTheory1, 2) / pi * 180 * 3600);
disp('初始粗对准DCM正交差RMS（″）');
disp(rms(dOrth1, 2) / pi * 180 * 3600);
disp('初始粗对准DCM规范差RMS（ppm）');
disp(rms(dNorm1, 2) * 1e6);

disp('采用g、g×ωIE为参考向量的解析粗对准：');
disp('实际姿态误差RMS（″）');
disp(rms(phiActual2, 2) / pi * 180 * 3600);
disp('理论与实际姿态误差差值RMS（″）');
disp(rms(phiActual2-phiTheory2, 2) / pi * 180 * 3600);
disp('初始粗对准DCM正交差RMS（″）');
disp(rms(dOrth2, 2) / pi * 180 * 3600);
disp('初始粗对准DCM规范差RMS（ppm）');
disp(rms(dNorm2, 2) * 1e6);